#pragma once
#include "axisgroup.h"
#include "pfsm_state.h"
#include <mutex>

namespace axisgroup {

class ForceControlIntf;
class PFsmState;
class OverPlan;

class Robot : public AxisGroup
{
public:
    struct MovingState
    {
        RobJointVal servo_cmd_pos;
        struct CmdState
        {
            RobJointVal cmd_pos;
            RobJointData<double> cmd_vel;
            RobJointData<double> cmd_acc;
            RobJointData<double> cmd_jerk;
            RobJointData<double> cmd_torque;
            ZucPose cmd_pose;
            double linear_cmd_vel;
            double linear_cmd_acc;
            double linar_cmd_jerk;
        } cmd;

        struct FdbState
        {
            RobJointVal fdb_pos;
            RobJointData<double> fdb_vel;
            RobJointData<double> fdb_acc;
            RobJointData<double> fdb_jerk;

            ZucPose fdb_pose;

            double linear_fdb_vel;
            RobJointData<double> fdb_torque;
            RobJointData<double> fdb_power;  // 机械功率
            RobJointData<double> fdb_momentum;
        } fdb;

        double Jacobian[MAX_AXIS * MAX_AXIS] = {};
    };

public:
    Robot(uint32_t serial_num = DEFAULT_SERIAL_NUM);
    virtual ~Robot();
    void update();

    void switch_plan_mode(PFsmState::PlanMode mode);  // 规划状态切换接口
    PFsmState::PlanMode get_cur_plan_mode() const;
    PFsmState::PlanMode get_target_plan_mode() const { return target_plan_mode_; }
    std::shared_ptr<axisgroup::PFsmState> get_free_pfsm() const { return free_pfsm_; }
    std::shared_ptr<axisgroup::PFsmState> get_drag_pfsm() const { return drag_pfsm_; }
    std::shared_ptr<axisgroup::PFsmState> get_coord_pfsm() const { return coord_pfsm_; }

    std::shared_ptr<axisgroup::PFsmState> get_servo_pfsm() const { return servo_pfsm_; }
    MovingState get_moving_state() const { return moving_state_; }
    bool is_inpos();
    void set_pos_jump() { pos_jump_ = true; }
    void motion_abort();  // 停止所有规划, CAT.2
    void pause();
    void resume();

public:
    void rt_over_plan_update(double ts);
    // 叠加功能 包含servo\力控\位置叠加\传送带跟踪等
    int update_pose_offset();

protected:
    bool is_singularity_ = false;

private:
    void init_pfsm();
    PFsmState::PlanMode target_plan_mode_ = PFsmState::PlanMode::PLAN_IDLE;
    std::shared_ptr<PFsmState> plan_pfsm_;
    std::shared_ptr<PFsmState> servo_pfsm_;
    std::shared_ptr<PFsmState> coord_pfsm_;
    std::shared_ptr<PFsmState> free_pfsm_;
    std::shared_ptr<PFsmState> drag_pfsm_;
    std::shared_ptr<PFsmState> idle_pfsm_;
    void update_plan_mode();
    void update_fsm_mode(PlanStatus state);
    int reset_kine_dyn() override;
    void update_fdb_state();
    void update_cmd_state();
    void update_cmd_torque();
    void watch_event();

public:
    std::shared_ptr<OverPlan> OverPlanItf() const { return over_plan_; };

public:
    void prepare_robot_status_for_task();
    void get_robot_status_for_task(mot::al::RobotALIntf::Status* status);

private:
    MovingState moving_state_;             // 规划状态统统使用该变量
    std::shared_ptr<OverPlan> over_plan_;  // 叠加规划
    bool pos_jump_ = false;                // 关闭叠加时，motion层的位置会与伺服层的位置有差异，需要同步，此时位置会有跳变

    // 力控
    std::shared_ptr<ForceControlIntf> ftc_;
    friend class ForceControlIntf;

public:
    static const char* get_plan_mode_stirng(PFsmState::PlanMode mode);
};
}  // namespace axisgroup
